#include "noeud.h"
#include "octree.h"
#include "ct_itemdrawable/ct_scene.h"
#include "ct_pointcloudindex/ct_pointcloudindexvector.h"
#include "ct_itemdrawable/tools/iterator/ct_groupiterator.h"
#include "ct_result/ct_resultgroup.h"
#include "ct_result/model/inModel/ct_inresultmodelgroup.h"

// Alias for indexing in models
#define DEF_resultIn_inputResult "inputResult"
#define DEF_groupIn_inputScene "inputGroup"
#define DEF_itemIn_scene "inputScene"
#define Max 8
#include <QDebug>
/*
Noeud::Noeud(){

}
*/
int count=0;
// Num fils = le noeud a creer est le "numFils"ieme fils du pere
Noeud::Noeud(Noeud* pere, unsigned int numFils, unsigned int *indPointDeb, unsigned int *indPointFin, CT_Scene* itemIn_scene)
{
        unsigned int i;
        Noeud * root;
        size_t nbPoints=itemIn_scene->getPointCloudIndexRegistered()->size();
        bool * deplX, * deplY, * deplZ,* outCodeFilsX, * outCodeFilsY, * outCodeFilsZ;
        CT_Point *outLeftBot;

        this->length=pere->length/2.0;
        this->depth=pere->depth+1;
        this->pere=pere;

        this->indPointDeb=indPointDeb;
        this->indPointFin=indPointFin;

        //nbPtsMin=pere->nbPtsMin;

        //Init Root

        float rootLength;
        if(itemIn_scene->maxX()>itemIn_scene->maxY())
            rootLength=itemIn_scene->maxX() - itemIn_scene->minX();
        else
            rootLength=itemIn_scene->maxY() - itemIn_scene->minY();
        if(rootLength<itemIn_scene->maxZ())
            rootLength=itemIn_scene->maxZ() - itemIn_scene->minZ();

        CT_Point leftBotRoot;

        leftBotRoot(0)=itemIn_scene->minX();
        leftBotRoot(1)=itemIn_scene->minY();
        leftBotRoot(2)=itemIn_scene->minZ();

        root= Noeud::createRoot(rootLength, 0, nbPoints, nbPtsMin, leftBotRoot);

        //Init Fils

        for(i=0;i<8;i++)
        {
            fils[i]=NULL;
        }

        //calcul code déplacement
        getCodeDeplacement(numFils,deplX,deplY,deplZ);

        //calcul codeBinX, codeBinY, codeBinZ
        codeBinFils(numFils, pere, deplX, deplY , deplZ, outCodeFilsX, outCodeFilsY, outCodeFilsZ );

        codeBinX=outCodeFilsX;
        codeBinY=outCodeFilsY;
        codeBinZ=outCodeFilsZ;

        //calcul de pointLeftBot
        getLeftPoint(pere, deplX, deplY, deplZ, outLeftBot);


}

Noeud *Noeud::createRoot(unsigned int length, unsigned int *indPointDeb, unsigned int *indPointFin, unsigned int nbPtsMin, CT_Point pointLeftBot)
{
    //en static dans le construct
    Noeud* root= new Noeud();
    root->depth=0;
    root->length=length;
    root->indPointDeb=indPointDeb;
    root->indPointFin=indPointFin;
    root->pere=NULL;
    root->pointLeftBot=pointLeftBot;
    root->codeBinX=0;
    root->codeBinY=0;
    root->codeBinZ=0;
    root->nbPtsMin=nbPtsMin;
    return root;
}

bool Noeud::isLeaf(Noeud* noeud)
{
    int i;
    for(i=0;i<8;i++)
    {
        if(noeud->fils[i]!=NULL)
            return false;
    }
    return true;

}

bool Noeud::isDivisible()
{
    if( (indPointFin - indPointDeb )+1 <= nbPtsMin){
        return false;
    }
    else
        return true;

}

void Noeud::subdivide(CT_ABSTRACT_MPCIR indexPointRe, Noeud* pere, CT_Scene* itemIn_scene)
{
    unsigned int *indDebut,*indFin,*a,*b,*c,*d,*e,*f,*g;
    float pivotX, pivotY, pivotZ;

    if(isDivisible()){

        //calcul PCA

        pivotX=pere->pointLeftBot(0)+pere->length/2;
        pivotY=pere->pointLeftBot(1)+pere->length/2;
        pivotZ=pere->pointLeftBot(2)+pere->length/2;

        //Tri
        sortPointsFromPivot2(indexPointRe,indDebut, indFin, pivotX,pivotY,pivotZ,a,b,c,d,e,f,g);

        fils[0]= new Noeud(pere, 0, pere->indPointDeb, a, itemIn_scene);
        fils[1]= new Noeud(pere, 1, a, b, itemIn_scene);
        fils[2]= new Noeud(pere, 2, b, c, itemIn_scene);
        fils[3]= new Noeud(pere, 3, c, d, itemIn_scene);
        fils[4]= new Noeud(pere, 4, d, e, itemIn_scene);
        fils[5]= new Noeud(pere, 5, e, f, itemIn_scene);
        fils[6]= new Noeud(pere, 6, f, g, itemIn_scene);
        fils[7]= new Noeud(pere, 7, g, pere->indPointFin, itemIn_scene);

        if(count==7)
        {
            count=0;
        }

        if(isLeaf(pere->fils[count]))
        {
           count++;
           subdivide(indexPointRe, pere->fils[count]);

        }
    }


}

void getCodeDeplacement(unsigned int nb, bool* deplX, bool* deplY, bool* deplZ )
{
    int i;
    bool* binaire;

    for (i=0;i<3;i++)
    {
        binaire[i]=(nb>>i)&1;
    }

    *deplX = binaire[0];
    *deplY = binaire[1];
    *deplZ = binaire[2];
}

void codeBinFils(unsigned int nb, Noeud* pere, bool deplX, bool deplY , bool deplZ, bool* outCodeFilsX, bool* outCodeFilsY, bool* outCodeFilsZ )
{

        *outCodeFilsX = ( pere->codeBinX | ( deplX << pere->depth ) );
        *outCodeFilsY = ( pere->codeBinY | ( deplY << pere->depth ) );
        *outCodeFilsZ = ( pere->codeBinZ | ( deplZ << pere->depth ) );

}

void getLeftPoint(Noeud* pere, bool deplX, bool deplY, bool deplZ, CT_Point* outLeftBot)
{
    (*outLeftBot)(0) = pere->pointLeftBot(0) + deplX * pere->length/2;
    (*outLeftBot)(1) = pere->pointLeftBot(1) + deplY * pere->length/2;
    (*outLeftBot)(2) = pere->pointLeftBot(2) + deplZ * pere->length/2;
}

void Noeud::sortPointsFromPivot2(CT_ABSTRACT_MPCIR indexPointRe, unsigned int *sortFirstIndex,unsigned int *sortLastIndex,float pivot1, float pivot2, float pivot3, unsigned int* a, unsigned int* b,unsigned int* c,unsigned int* d,unsigned int* e,unsigned int* f,unsigned int* g)
{
    CT_ABSTRACT_MPCIR indexPointReCopy;
    size_t nbPoints = indexPointRe->size();
    *sortFirstIndex=pere->indPointDeb;
    *sortLastIndex=pere->indPointFin;


    qDebug()<<"test";

    *d=sortPointsFromPivot( indexPointRe, *sortFirstIndex, *sortLastIndex, pivot3, 2 );
   // qDebug()<<"fin 1" <<sortFirstIndex << *d << sortLastIndex;
    *b=sortPointsFromPivot( indexPointRe, *sortFirstIndex, *d, pivot2, 1 );
   // qDebug()<<"fin 2" <<sortFirstIndex << *b <<*d;
    *f=sortPointsFromPivot( indexPointRe, *d, *sortLastIndex, pivot2, 1 );
   // qDebug()<<"fin 3" <<*d+1<< *f<<sortLastIndex;
    *a=sortPointsFromPivot( indexPointRe, *sortFirstIndex, *b, pivot1, 0 );
   // qDebug()<<"fin 4" <<sortFirstIndex<<*a<<*b ;
    *c=sortPointsFromPivot( indexPointRe, *b, *d, pivot1, 0 );
   // qDebug()<<"fin 5"<<*b+1<<*c<<*d;
    *e=sortPointsFromPivot( indexPointRe, *d, *f, pivot1, 0 );
   // qDebug()<<"fin 6"<<*d+1<<*e<<*f;
    *g=sortPointsFromPivot( indexPointRe, *f,*sortLastIndex, pivot1, 0 );
   // qDebug()<<"fin 7"<<*e<<*f+1<<*g;

    qDebug()<<a <<b <<c << d<< e<< f<< g;


}

unsigned int Noeud::sortPointsFromPivot(CT_ABSTRACT_MPCIR outSortedIndexCloudPtr,unsigned int sortFirstIndex,unsigned int sortLastIndex,float pivot,int sortAxis )
{
        unsigned int perm;

        if ( sortLastIndex == sortFirstIndex )
        {
            return sortLastIndex;
        }

        if (sortLastIndex==sortFirstIndex+1)
        {
            if(outSortedIndexCloudPtr->tAt(sortFirstIndex)(sortAxis)<pivot){
                qDebug()<<"retour"<<sortLastIndex;
                return sortLastIndex;

            }

            else{

                qDebug()<<"retour"<<sortFirstIndex;
                return sortFirstIndex;
            }
        }

//        else if (sortLastIndex - sortFirstIndex==1)
//        {
//        }
        else
        {
            unsigned int firstIndexCopie=sortFirstIndex, lastIndexCopie=sortLastIndex-1;

            while(firstIndexCopie<lastIndexCopie){

               while(firstIndexCopie<lastIndexCopie && outSortedIndexCloudPtr->tAt(firstIndexCopie)(sortAxis)<pivot)
               {
                     firstIndexCopie++;
               }
               while(firstIndexCopie<lastIndexCopie && outSortedIndexCloudPtr->tAt(lastIndexCopie)(sortAxis)>=pivot)
               {
                      lastIndexCopie--;
               }


               //on permute
          //     if(sortFirstIndex<sortLastIndex)
          //     {
                   //printDebug(outSortedIndexCloudPtr);

                            qDebug()<<"sortLastIndex"<<lastIndexCopie<<"sortFirstIndex"<<firstIndexCopie;

                   perm=outSortedIndexCloudPtr->indexAt(firstIndexCopie);
                             qDebug()<<perm;
                   outSortedIndexCloudPtr->replaceIndexAt(firstIndexCopie, outSortedIndexCloudPtr->indexAt(lastIndexCopie));
                                qDebug()<<outSortedIndexCloudPtr->indexAt(firstIndexCopie);
                   outSortedIndexCloudPtr->replaceIndexAt(lastIndexCopie, perm);
                                qDebug()<<outSortedIndexCloudPtr->indexAt(lastIndexCopie);
                  // sortFirstIndex++;
                  // sortLastIndex--;

           //    }

        }
       /* if(sortFirstIndex=sortLastIndex){
            if(outSortedIndexCloudPtr->tAt(sortFirstIndex)(sortAxis)<pivot)
                return sortFirstIndex;
            else
                return sortFirstIndex-1;
        }*/
        if(firstIndexCopie==sortLastIndex-1){
            if(outSortedIndexCloudPtr->tAt(sortLastIndex-1)(sortAxis)<pivot)
                return sortLastIndex;
            else
                return sortLastIndex-1;
        }

        return firstIndexCopie;
    }
}

void Noeud::printDebug(CT_ABSTRACT_MPCIR indexPoint){
    size_t nbPts=indexPoint->size();
    int i;
    qDebug()<<"_______________________";
    for (i=0;i<nbPts;i++){
        qDebug()<<indexPoint->indexAt(i)<<indexPoint->tAt(i)(0)<<indexPoint->tAt(i)(1)<<indexPoint->tAt(i)(2);
    }

}
